//===========================================================================
/*
    This file is part of the CHAI 3D visualization and haptics libraries.
    Copyright (C) 2003-2009 by CHAI 3D. All rights reserved.

    This library is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License("GPL") version 2
    as published by the Free Software Foundation.

    For using the CHAI 3D libraries with software that can not be combined
    with the GNU GPL, and for taking advantage of the additional benefits
    of our support services, please contact CHAI 3D about acquiring a
    Professional Edition License.

    \author    <http://www.chai3d.org>
    \author    Francois Conti
    \version   2.0.0 $Rev: 269 $
*/
//===========================================================================

//---------------------------------------------------------------------------
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>
#include <sstream>
//---------------------------------------------------------------------------
#include "chai3d.h"
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// DECLARED CONSTANTS
//---------------------------------------------------------------------------

// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W         = 600;
const int WINDOW_SIZE_H         = 600;

// mouse menu options (right button)
const int OPTION_FULLSCREEN     = 1;
const int OPTION_WINDOWDISPLAY  = 2;

// size of map
const double MESH_SCALE_SIZE = 2.0;


//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera that renders the world in a window display
cCamera* camera;

// a mesh object used to create the height map
cMesh* object;

// a small magnetic line used to constrain the tool alon the vertical axis
cShapeLine* magneticLine;

// two sphere position at the end of the magnetic line
cShapeSphere* sphereA;
cShapeSphere* sphereB;

// a light source to illuminate the objects in the virtual scene
cLight *light;

// a little "chai3d" bitmap logo at the bottom of the screen
//cBitmap* logo;

// width and height of the current window display
int displayW  = 0;
int displayH  = 0;

// a haptic device handler
cHapticDeviceHandler* handler;

// a haptic deviece info
cHapticDeviceInfo info;

//
cGenericHapticDevice* hapticDevice;

// a 3D cursor which represents the haptic device
cGeneric3dofPointer* tool;

// Avatar to follow
cShapeSphere* cursor;

// radius of the tool proxy
double proxyRadius;

// status of the main simulation haptics loop
bool simulationRunning = false;

// update camera settings
void updateCameraPosition();

// camera position and orientation is spherical coordinates
double cameraAngleH;
double cameraAngleV;
double cameraDistance;
cVector3d cameraPosition;

// Tool start coordinates
double tx = 0.0;
double ty = 0.0;
double tz = 0.1;

// Tool global coordinates (needed by updateGraphics)
cVector3d toolGlobalPos;
cVector3d prevToolGlobalPos;

// Cursor start coordinates
double cx = 0.0;
double cy = 0.0;
double cz = 0.1;

// Some cursor data
double		 cursorSpeed		   = 0.0001	     ;
double**	 route				   = 0			 ;
unsigned int routeLength		   = 0			 ;
unsigned int cursorCurrentLocation = 0			 ;
double		 velocity[2]		   = { 0.0, 0.0 };

double diff = 0;

// Elapsed time
ULONGLONG timeStart		 = 0;		// Start of simulation
ULONGLONG timeGraphics	 = 0;		// Ellapsed time (graphics loop updated)
ULONGLONG timePrevFrame	 = 0;		// Time of previus frame
ULONGLONG timeHapticPrev = 0;		// Time of previus haptic iteration
ULONGLONG timeHaptic	 = 0;		// Ellapsed time (haptic loop updated)

// Current frame
int	   frame = 0;
double fps = 0.0;

// camera status
bool flagCameraInMotion;

// mouse position and button status
int mouseX, mouseY, mouseButton;

// root resource path
string resourceRoot;

// has exited haptics simulation thread
bool simulationFinished = false;

// label for position
cLabel* positionLabel;
cLabel* cursorLabel;
cLabel* fpsLabel;
cLabel* timeLabel;
cLabel* velocityLabel;

cGenericObject* rootLabels;

//---------------------------------------------------------------------------
// DECLARED MACROS
//---------------------------------------------------------------------------
// convert to resource path
#define RESOURCE_PATH(p)    (char*)((resourceRoot+string(p)).c_str())


//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);

// callback to handle mouse click
void mouseClick(int button, int state, int x, int y);

// callback to handle mouse motion
void mouseMove(int x, int y);

// function called before exiting the application
void close(void);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);

// test if a pixel is neightbor of another
bool neightbor(int i, int j);

// loads a bitmap file and create 3D height map based on pixel color
int loadHeightMap();

// returns current system time in ms
inline ULONGLONG time_ms();

//===========================================================================
/*
    DEMO:    map.cpp

    This example illustrates the construction a triangle based object.
    The application first loads a bitmap texture image. For each pixel,
    we then define a height by computing the gray-scale value. A vertex
    is created for each pixel and triangles are then generated to connect
    the array of vertices together. This example also demonstrates the
    use of mouse callback commands to allow the operator to control the
    position of the virtual camera. The operator can also use the haptic
    device (user switch command) to move the camera or grasp a point on the 
    surface and deform the terrain.

    In the main haptics loop function  "updateHaptics()" , the position
    of the haptic device is retrieved at each simulation iteration.
    The interaction forces are then computed and sent to the device.
*/
//===========================================================================

int main(int argc, char* argv[])
{
	//-----------------------------------------------------------------------
	// INITIALIZATION
	//-----------------------------------------------------------------------
	
	printf ("\n");
	printf ("-----------------------------------\n");
	printf ("CHAI 3D\n");
	printf ("Demo: 20-map\n");
	printf ("Copyright 2003-2009\n");
	printf ("-----------------------------------\n");
	printf ("\n\n");
	printf ("Instructions:\n\n");
	printf ("- Use left mouse button to rotate camera view. \n");
	printf ("- Use right mouse button to control camera zoom. \n");
	printf ("- Use haptic device and user switch to rotate \n");
	printf ("  camera or deform terrain. \n");
	printf ("\n\n");
	printf ("Keyboard Options:\n\n");
	printf ("[1] - Texture   (ON/OFF)\n");
	printf ("[2] - Wireframe (ON/OFF)\n");
	printf ("[x] - Exit application\n");
	printf ("\n\n");
	
	
	// parse first arg to try and locate resources
	resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);

	//-----------------------------------------------------------------------
	// 3D - SCENEGRAPH
	//-----------------------------------------------------------------------
	
	// create a new world.
	world = new cWorld();
	
	// set the background color of the environment
	// the color is defined by its (R,G,B) components.
	world->setBackgroundColor(0.1, 0.1, 0.1);
	
	// create a camera and insert it into the virtual world
	camera = new cCamera(world);
	world->addChild(camera);
	
	// define a default position of the camera (described in spherical coordinates)
	cameraAngleH = 0;
	cameraAngleV = 90;
	cameraDistance = 1.8 * MESH_SCALE_SIZE;
	updateCameraPosition();
	
	// set the near and far clipping planes of the camera
	// anything in front/behind these clipping planes will not be rendered
	camera->setClippingPlanes(0.01, 10.0);
	
	// create a light source and attach it to the camera
	light = new cLight(world);
	camera->addChild(light);					// attach light to camera
	light->setEnabled(true);					// enable light source
	light->setPos(cVector3d( 0.0, 0.3, 0.3));	// position the light source
	light->setDir(cVector3d(-1.0,-0.1, -0.1));  // define the direction of the light beam
	light->m_ambient.set(0.5, 0.5, 0.5);
	light->m_diffuse.set(0.8, 0.8, 0.8);
	light->m_specular.set(1.0, 1.0, 1.0);
	
	//-----------------------------------------------------------------------
	// HAPTIC DEVICES / TOOLS
	//-----------------------------------------------------------------------
	
	// create a haptic device handler
	handler = new cHapticDeviceHandler();
	
	// get access to the first available haptic device
	handler->getDevice(hapticDevice, 0);
	
	// open connection to haptic device
    hapticDevice->open();

	// initialize haptic device
	hapticDevice->initialize();

	// retrieve information about the current haptic device
	if (hapticDevice)
	{
	    info = hapticDevice->getSpecifications();
	}
		
	// create a 3D tool and add it to the world
	tool = new cGeneric3dofPointer(world);
	
	// attach tool to camera
	world->addChild(tool);
	
	// position tool workspace in front of camera (x-axis of camera reference pointing towards the viewer)
	//tool->setPos( tx, ty, tz );
	
	// connect the haptic device to the tool
	tool->setHapticDevice(hapticDevice);
	
	// initialize tool by connecting to haptic device
	tool->start();
	
	// map the physical workspace of the haptic device to a larger virtual workspace.
	tool->setWorkspaceRadius(8.0);
	
	// define a radius for the tool (graphical display)
	tool->setRadius(0.02);
	
	// hide the device sphere. only show proxy.
	tool->m_deviceSphere->setShowEnabled(false);
	
	// set the physical readius of the proxy.
	proxyRadius = 0.0;
	tool->m_proxyPointForceModel->setProxyRadius(proxyRadius);
	tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
	
	cursor = new cShapeSphere(0.02);
	world->addChild(cursor);
	cursor->setHapticEnabled(false);
	
	//-----------------------------------------------------------------------
	// COMPOSE THE VIRTUAL SCENE
	//-----------------------------------------------------------------------
	// create a new mesh to display a height map
	object = new cMesh(world);
	world->addChild(object);
	
	// load default map
	loadHeightMap();
	
	// position tool workspace in front of camera (x-axis of camera reference pointing towards the viewer)
	tool->setPos( tx, ty, tz );
	cursor->setPos( cx, cy, cz );
	
	// read the scale factor between the physical workspace of the haptic
	// device and the virtual workspace defined for the tool
	double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
	
	// define a maximum stiffness that can be handled by the current
	// haptic device. The value is scaled to take into account the
	// workspace scale factor
	double stiffnessMax = info.m_maxForceStiffness / workspaceScaleFactor;
	object->setStiffness(0.5 * stiffnessMax, true);
	
	// create a small vertical white magnetic line that will be activated when the
	// user deforms the mesh.
	magneticLine = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
	world->addChild(magneticLine);
	magneticLine->m_ColorPointA.set(0.6, 0.6, 0.6);
	magneticLine->m_ColorPointB.set(0.6, 0.6, 0.6);
	magneticLine->setShowEnabled(false);
	
	// set haptic properties
	magneticLine->m_material.setStiffness(0.05 * stiffnessMax);
	magneticLine->m_material.setMagnetMaxForce(0.2 * info.m_maxForce);
	magneticLine->m_material.setMagnetMaxDistance(0.25);
	magneticLine->m_material.setViscosity(0.05 * info.m_maxLinearDamping);
	
	// create a haptic magnetic effect
	cEffectMagnet* newEffect = new cEffectMagnet(magneticLine);
	magneticLine->addEffect(newEffect);
	
	// disable haptic feedback for now
	magneticLine->setHapticEnabled(false);
	
	// create two sphere that will be added at both ends of the line
	sphereA = new cShapeSphere(0.02);
	sphereB = new cShapeSphere(0.02);
	world->addChild(sphereA);
	world->addChild(sphereB);
	sphereA->setShowEnabled(false);
	sphereB->setShowEnabled(false);
	
	// define some material properties for spheres
	cMaterial matSphere;
	matSphere.m_ambient.set(0.5, 0.2, 0.2);
	matSphere.m_diffuse.set(0.8, 0.4, 0.4);
	matSphere.m_specular.set(1.0, 1.0, 1.0);
	sphereA->setMaterial(matSphere);
	sphereB->setMaterial(matSphere);
	
	rootLabels = new cGenericObject();
	camera->m_front_2Dscene.addChild(rootLabels);
	positionLabel = new cLabel();
	rootLabels->addChild(positionLabel);
	
	positionLabel->setPos(0, -20.0, 0);
	positionLabel->m_fontColor.set(0.6, 0.6, 0.6);
	
	cursorLabel = new cLabel();
	rootLabels->addChild(cursorLabel);
	
	cursorLabel->setPos(0, -40.0, 0);
	cursorLabel->m_fontColor.set(0.6, 0.6, 0.6);
	
	velocityLabel = new cLabel();
	rootLabels->addChild(velocityLabel);
	
	velocityLabel->setPos(0, -60.0, 0);
	velocityLabel->m_fontColor.set(0.6, 0.6, 0.6);
	
	fpsLabel = new cLabel();
	rootLabels->addChild(fpsLabel);
	
	fpsLabel->setPos(0, 0, 0);
	fpsLabel->m_fontColor.set(0.8, 0.8, 0.8);
	
	timeLabel = new cLabel();
	rootLabels->addChild(timeLabel);
	
	timeLabel->setPos(WINDOW_SIZE_W - 100, 0, 0);
	timeLabel->m_fontColor.set(0.8, 0.8, 0.8);
	
	//-----------------------------------------------------------------------
	// OPEN GL - WINDOW DISPLAY
	//-----------------------------------------------------------------------
	
	// initialize GLUT
	glutInit(&argc, argv);
	
	// retrieve the resolution of the computer display and estimate the position
	// of the GLUT window so that it is located at the center of the screen
	int screenW = glutGet(GLUT_SCREEN_WIDTH);
	int screenH = glutGet(GLUT_SCREEN_HEIGHT);
	int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
	int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
	
	// initialize the OpenGL GLUT window
	glutInitWindowPosition(windowPosX, windowPosY);
	glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
	glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
	glutCreateWindow(argv[0]);
	glutDisplayFunc(updateGraphics);
	glutMouseFunc(mouseClick);
	glutMotionFunc(mouseMove);
	glutKeyboardFunc(keySelect);
	glutReshapeFunc(resizeWindow);
	glutSetWindowTitle("CHAI 3D");
	
	
	//-----------------------------------------------------------------------
	// START SIMULATION
	//-----------------------------------------------------------------------
	
	// simulation in now running
	simulationRunning = true;
	
	// create a thread which starts the main haptics rendering loop
	cThread* hapticsThread = new cThread();
	hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
	
	timeStart = timeGraphics = timePrevFrame = timeHapticPrev = timeHaptic = time_ms();
	
	// start the main graphics rendering loop
	glutMainLoop();
	
	// close everything
	close();
	
	// exit
	return (0);
}

//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
    // update the size of the viewport
    displayW = w;
    displayH = h;
    glViewport(0, 0, displayW, displayH);

	rootLabels->setPos(10, displayH-10, 0);
	timeLabel->setPos(displayW-100, 0, 0);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
    // escape key
    if ((key == 27) || (key == 'x'))
    {
        // close everything
        close();

        // exit application
        exit(0);
    }

    // option 1:
    if (key == '1')
    {
        bool useTexture = object->getUseTexture();
        object->setUseTexture(!useTexture);
    }

    // option 2:
    if (key == '2')
    {
        bool useWireMode = object->getWireMode();
        object->setWireMode(!useWireMode);
    }
}

//---------------------------------------------------------------------------

void mouseClick(int button, int state, int x, int y)
{
     // mouse button down
    if (state == GLUT_DOWN)
    {
        flagCameraInMotion = true;
        mouseX = x;
        mouseY = y;
        mouseButton = button;
    }

    // mouse button up
    else if (state == GLUT_UP)
    {
        flagCameraInMotion = false;
    }
}

//---------------------------------------------------------------------------

void mouseMove(int x, int y)
{
    if (flagCameraInMotion)
    {
        if (mouseButton == GLUT_RIGHT_BUTTON)
        {
            cameraDistance = cameraDistance - 0.01 * (y - mouseY);
        }

        else if (mouseButton == GLUT_LEFT_BUTTON)
        {
            cameraAngleH = cameraAngleH - (x - mouseX);
            cameraAngleV = cameraAngleV + (y - mouseY);
        }
    }

    updateCameraPosition();

    mouseX = x;
    mouseY = y;
}

//---------------------------------------------------------------------------

void close(void)
{
    // stop the simulation
    simulationRunning = false;

    // wait for graphics and haptics loops to terminate
    while (!simulationFinished) { cSleepMs(100); }

    // close haptic device
    tool->stop();
}

//---------------------------------------------------------------------------

void updateGraphics(void)
{
	frame++;

	timeGraphics = time_ms();

	if( frame == 1 )
		timePrevFrame = timeGraphics ;
	else if ( frame == 30 )
	{
		frame = 0;
		fps = 30000.0 / (double)(timeGraphics-timePrevFrame);
	}
	
	string strLabel;

	cStr(strLabel,fps);

	fpsLabel->m_string = strLabel + " FPS";
	
	ULONGLONG tmp = timeGraphics - timeStart;
	
	unsigned int minutes = tmp/60000;
	unsigned int milisec = (tmp%60000);
	unsigned int seconds = milisec/1000;
	milisec = milisec % 1000;

	// update content of position label
	// read position of device an convert into millimeters
	cVector3d pos;
	pos = toolGlobalPos;
	pos.mul(1000);
	
	// create a string that concatenates the device number and its position.
	strLabel.assign("Tool   - x: ");
	cStr(strLabel, pos.x, 2);
	strLabel = strLabel + "   y: ";
	cStr(strLabel, pos.y, 2);
	strLabel = strLabel + "   z: ";
	cStr(strLabel, pos.z, 2);
	
	positionLabel->m_string = strLabel;

	strLabel.clear();
	strLabel.assign("Cursor -  x: ");
	cStr(strLabel, cx, 2);
	strLabel = strLabel + "   y: ";
	cStr(strLabel, cy, 2);
	strLabel = strLabel + "   z: ";
	cStr(strLabel, cz, 2);

	cursorLabel->m_string = strLabel;

	strLabel.clear();
	strLabel.assign("Vel   vx: ");
	cStr(strLabel, velocity[0], 2);
	strLabel = strLabel + "  vy: ";
	cStr(strLabel, velocity[1], 2);
	
	velocityLabel->m_string = strLabel;

	strLabel.clear();


	char buffer[65];
	
	_ui64toa(minutes,buffer,10);
	strLabel.append(buffer);
	strLabel.append(":");

	_ui64toa(seconds,buffer,10);
	if(seconds < 10 )
		strLabel.append("0");
	strLabel.append(buffer);
	strLabel.append(":");

	_ui64toa(milisec,buffer,10);
	if(milisec < 10 )
		strLabel.append("00");
	else if(milisec < 100 )
		strLabel.append("0");
	strLabel.append(buffer);

	timeLabel->m_string = strLabel;
	
	// update object normals
	object->computeAllNormals(true);
	
	// render world
	camera->renderView(displayW, displayH);
	
	// Swap buffers
	glutSwapBuffers();
	
	// check for any OpenGL errors
	GLenum err;
	err = glGetError();
	if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));
	
	// inform the GLUT window to call updateGraphics again (next frame)
	if (simulationRunning)
	{
	    glutPostRedisplay();
	}
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
	// state machine
	const int STATE_IDLE            = 1;
	const int STATE_MODIFY_MAP      = 2;
	const int STATE_MOVE_CAMERA     = 3;
	int state = STATE_IDLE;  
	
	// current tool position
	cVector3d toolLocalPos;         // local coordinates
	
	// previous tool position
	cVector3d prevToolLocalPos;     // local coordinates
	
	// main haptic simulation loop
	while(simulationRunning)
	{
		timeHaptic = time_ms() ;
	
		if(timeHapticPrev>=timeHaptic)
			continue;
		
	    // compute global reference frames for each object
	    world->computeGlobalPositions(true);
	
	    // update position and orientation of tool
	    tool->updatePose();
	
	    // compute interaction forces
	    tool->computeInteractionForces();
	
		// update cursor
		
		if ((cursorCurrentLocation+1) < routeLength)
		{
			if ( (  ( velocity[0] > 0 && cx > route[0][cursorCurrentLocation+1] ) || ( velocity[0] < 0 && cx < route[0][cursorCurrentLocation+1] ) || ( velocity[0] == 0 )  ) &&
				 (  ( velocity[1] > 0 && cy > route[1][cursorCurrentLocation+1] ) || ( velocity[1] < 0 && cy < route[1][cursorCurrentLocation+1] ) || ( velocity[1] == 0 )  )
			)
			{
				cursorCurrentLocation++;
				if( (cursorCurrentLocation+1) < routeLength )
				{
					velocity[0] = route[0][cursorCurrentLocation+1]-route[0][cursorCurrentLocation];
					velocity[1] = route[1][cursorCurrentLocation+1]-route[1][cursorCurrentLocation];
					double m = sqrt(velocity[0]*velocity[0] + velocity[1]*velocity[1]);
					velocity[0] /= m;
					velocity[1] /= m;
					if(abs(velocity[0])!=1 &&abs(velocity[0])!=0)
						int dummy=0;
				}
				else
				{
					velocity[0] = 0.0;
					velocity[1] = 0.0;
				}
			}
		}
		
		cx += ((double)(timeHaptic-timeHapticPrev))*cursorSpeed*velocity[0];
		cy += ((double)(timeHaptic-timeHapticPrev))*cursorSpeed*velocity[1];
	
		cursor->setPos( cx, cy, cz );
	    
		// read user switch
	    bool userSwitch = tool->getUserSwitch(0);
	
	    // update tool position
	    toolGlobalPos = tool->getDeviceGlobalPos();
	    toolLocalPos  = tool->getDeviceLocalPos();
	
	    if ((state == STATE_MOVE_CAMERA) && (!userSwitch))
	    {
	        state = STATE_IDLE;
	
	        // enable haptic interaction with map
	        object->setHapticEnabled(true, true);
	    }
	
	    else if (((state == STATE_MODIFY_MAP) && (!userSwitch)) ||
	        ((state == STATE_MODIFY_MAP) && (!tool->isInContact(magneticLine))))
	    {
	        state = STATE_IDLE;
	
	        // disable magnetic line
	        magneticLine->setHapticEnabled(false);
	        magneticLine->setShowEnabled(false);
	
	        // disable spheres
	        sphereA->setShowEnabled(false);
	        sphereB->setShowEnabled(false);
	
	        // enable haptic interaction with map
	        object->setHapticEnabled(true, true);
	
	        // disable forces
	        tool->setForcesOFF();
	
	        // update bounding box (can take a little time)
	        object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
	        
	        // enable forces again
	        tool->setForcesON();
	    }
	
	    // user clicks with the mouse
	    else if ((state == STATE_IDLE) && (userSwitch))
	    {
	        // start deforming object
	        if (tool->isInContact(object))
	        {
	            state = STATE_MODIFY_MAP;
	
	            // update position of line
	            cVector3d posA = toolGlobalPos;
	            posA.z =-0.7;
	
	            cVector3d posB = toolGlobalPos;
	            posB.z = 0.7;
	
	            magneticLine->m_pointA = posA;
	            magneticLine->m_pointB = posB;
	
	            // update position of spheres
	            sphereA->setPos(posA);
	            sphereB->setPos(posB);
	
	            // enable spheres
	            sphereA->setShowEnabled(true);
	            sphereB->setShowEnabled(true);
	
	            // enable magnetic line
	            magneticLine->setHapticEnabled(true);
	            magneticLine->setShowEnabled(true);
	
	            // disable haptic interaction with map
	            object->setHapticEnabled(false, true);
	        }
	
	        // start moving camera
	        else
	        {
	            state = STATE_MOVE_CAMERA;
	            
	            // disable haptic interaction with map
	            object->setHapticEnabled(false, true);
	        }
	    }
	
	    // modify map
	    else if (state == STATE_MODIFY_MAP)
	    {
	        // compute tool offset
	        cVector3d offset = toolGlobalPos - prevToolGlobalPos;
	
	        // map offset on z axis
	        double offsetHeight = offset.z;
	
	        // apply offset to all vertices through a weighted function
	        int numVertices = object->getNumVertices(true);
	        for (int i=0; i<numVertices; i++)
	        {
	            // get next vertex
	            cVertex* vertex = object->getVertex(i, true);
	
	            // compute distance between vertex and tool
	            cVector3d posTool = tool->getProxyGlobalPos();
	            cVector3d posVertex = vertex->getPos();
	            double distance = cDistance(posTool, posVertex);
	
	            // compute factor
	            double RADIUS = 0.4;
	            double relativeDistance = distance / RADIUS;
	            double clampedRelativeDistance = cClamp01(relativeDistance);
	            double w = 0.5 + 0.5 * cos(clampedRelativeDistance * CHAI_PI);
	
	            // apply offset
	            double offsetVertexHeight = w * offsetHeight;
	            posVertex.z = posVertex.z + offsetVertexHeight;
	            vertex->setPos(posVertex);
	        }
	    }
	
	    // move camera
	    else if (state == STATE_MOVE_CAMERA)
	    {
	        // compute tool offset
	        cVector3d offset = toolLocalPos - prevToolLocalPos;
	
	        // apply camera motion
	        cameraDistance = cameraDistance - 2 * offset.x;
	        cameraAngleH = cameraAngleH - 40 * offset.y;
	        cameraAngleV = cameraAngleV - 40 * offset.z;
	
	        updateCameraPosition();   
	    }
	
	    // store tool position
	    prevToolLocalPos  = toolLocalPos;
	    prevToolGlobalPos = toolGlobalPos;
	
	    // send forces to device
	    tool->applyForces();
		timeHapticPrev = timeHaptic;
	}
	
	// exit haptics thread
	simulationFinished = true;
}

//---------------------------------------------------------------------------

int loadHeightMap()
{
    // create a texture file
    cTexture2D* newTexture = new cTexture2D();
    world->addTexture(newTexture);

    // texture 2D
    bool fileload = newTexture->loadFromFile(RESOURCE_PATH("resources\\images\\maze32.bmp"));
    if (!fileload)
    {
        #if defined(_MSVC)
        fileload = newTexture->loadFromFile("C:\\Users\\figi\\Documents\\HARD\\bin\\resources\\images\\maze32.bmp");
        #endif
    }
    if (!fileload)
    {
        printf("Error - Texture image failed to load correctly.\n");
        close();
        return (-1);
    }

    // get the size of the texture image (U and V)
    int texSizeU = newTexture->m_image.getWidth();
    int texSizeV = newTexture->m_image.getHeight();

    // check size of image
    if ((texSizeU < 1) || (texSizeV < 1)) { return (false); }

    // we look for the largest side
    int largestSide;
    if (texSizeU > texSizeV)
    {
        largestSide = texSizeU;
    }
    else
    {
        largestSide = texSizeV;
    }

    // The largest side of the map has a length of 1.0
    // we now compute the respective size for 1 pixel of the image in world space.
    double size = 1.0 / (double)largestSide;

    // we will create an triangle based object. For centering puposes we
    // compute an offset for axis X and Y corresponding to the half size
    // of the image map.
    double offsetU = 0.5 * (double)texSizeU * size;
    double offsetV = 0.5 * (double)texSizeV * size;

	//Create table with cursor route
	route = new double*[2];
	route[0] = new double[(texSizeU*texSizeV)/2];
	route[1] = new double[(texSizeU*texSizeV)/2];

    // For each pixel of the image, create a vertex
    int u,v;
	double goalX, goalY;
    for (v=0; v<texSizeV; v++)
    {
        for (u=0; u<texSizeU; u++)
        {
            double px, py, tu, tv, height;

            // compute the position of the vertex
            px = size * (double)u - offsetU;
            py = size * (double)v - offsetV;

			// compute the height of the vertex
            cColorb color = newTexture->m_image.getPixelColor(u,v);

			double r = (double)color.getR();
			double g = (double)color.getG();
			double b = (double)color.getB();

			if ( (double)color.getR() ==   0.0 && 
				 (double)color.getG() == 255.0 &&
				 (double)color.getB() ==   0.0    )				// Start
			{
				tx = px*MESH_SCALE_SIZE;
				ty = py*MESH_SCALE_SIZE;
				cColorb a(0x00,0x00,0x00);
				newTexture->m_image.setPixelColor(u,v,a);
				height = 0;
			}
			else if ( (double)color.getR() == 255.0 && 
					  (double)color.getG() ==   0.0 &&
					  (double)color.getB() ==   0.0    )		// Goal
			{
				goalX = px;
				goalY = py;
				cColorb a(0x00,0x00,0x00);
				newTexture->m_image.setPixelColor(u,v,a);
				height = 0;
			}
			else if ( (double)color.getR() == 255.0 && 
					  (double)color.getG() == 255.0 &&
					  (double)color.getB() ==   0.0    )		// Path
			{
				cColorb a(0x00,0x00,0x00);
				newTexture->m_image.setPixelColor(u,v,a);
				routeLength++;
				route[0][routeLength] = u;
				route[1][routeLength] = v;
				height = 0;
			}
			else if ( (double)color.getR() ==   0.0 && 
					  (double)color.getG() == 255.0 &&
					  (double)color.getB() == 255.0    )		// Cursor start
			{
				cx = px*MESH_SCALE_SIZE;
				cy = py*MESH_SCALE_SIZE;
				route[0][0] = u;
				route[1][0] = v;
				cColorb a(0x00,0x00,0x00);
				newTexture->m_image.setPixelColor(u,v,a);
				height = 0;
			}
			else
				height = 0.1 * ((double)color.getR() + (double)color.getG() + (double)color.getB()) / (3.0 * 255.0);

            // create new vertex
            unsigned int index = object->newVertex(px, py, height);
            cVertex* vertex = object->getVertex(index);

            // compute texture coordinate
            tu = (double)u / texSizeU;
            tv = (double)v / texSizeV;
            vertex->setTexCoord(tu, tv);
        }
    }
	routeLength++;
	for ( unsigned int i=0; i<routeLength ; i++ )
		for ( unsigned int j=i+1; j<routeLength ; j++ )
			if(neightbor(i,j))
			{
				double tmp[2];
				tmp[0] = route[0][i+1];
				tmp[1] = route[1][i+1];
				route[0][i+1] = route[0][j];
				route[1][i+1] = route[1][j];
				route[0][j] = tmp[0];
				route[1][j] = tmp[1];
			}
	for ( unsigned int i=0; i<routeLength ; i++ )
	{
		route[0][i] = (size*route[0][i]-offsetU)*MESH_SCALE_SIZE;
		route[1][i] = (size*route[1][i]-offsetV)*MESH_SCALE_SIZE;
	}
	routeLength++;
	route[0][routeLength-1] = goalX*MESH_SCALE_SIZE;
	route[1][routeLength-1] = goalY*MESH_SCALE_SIZE;

	//Set first velocity vector
	velocity[0] = route[0][1]-route[0][0];
	velocity[1] = route[1][1]-route[1][0];
	double m = sqrt(velocity[0]*velocity[0] + velocity[1]*velocity[1]);
	velocity[0] /= m;
	velocity[1] /= m;

	// print route
	printf("route: \n");
	for ( unsigned int i=0; i<routeLength ; i++ )
		printf("%i: %.4f, %.4f\n", i, (float)route[0][i], (float)route[1][i]);
	
    // Create a triangle based map using the above pixels
	for (v=0; v<(texSizeV-1); v++)
	{
	    for (u=0; u<(texSizeU-1); u++)
	    {
	        // get the indexing numbers of the next four vertices
	        unsigned int index00 = ((v + 0) * texSizeU) + (u + 0);
	        unsigned int index01 = ((v + 0) * texSizeU) + (u + 1);
	        unsigned int index10 = ((v + 1) * texSizeU) + (u + 0);
	        unsigned int index11 = ((v + 1) * texSizeU) + (u + 1);
	
	        // create two new triangles
	        object->newTriangle(index00, index01, index10);
	        object->newTriangle(index10, index01, index11);
	    }
	}
	// apply texture to object
	object->setTexture(newTexture);
	object->setUseTexture(true);
	
	// compute normals
	object->computeAllNormals(true);
	
	// compute size of object
	object->computeBoundaryBox(true);
	
	cVector3d min = object->getBoundaryMin();
	cVector3d max = object->getBoundaryMax();
	
	// This is the "size" of the object
	cVector3d span = cSub(max, min);
	size = cMax(span.x, cMax(span.y, span.z));
	
	// We'll center all vertices, then multiply by this amount,
	// to scale to the desired size.
	double scaleFactor = MESH_SCALE_SIZE / size;
	object->scale(scaleFactor);
	
	// compute size of object again
	object->computeBoundaryBox(true);
	
	// Build a collision-detector for this object, so
	// the proxy will work nicely when haptics are enabled.
	object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
	
	// set size of frame
	object->setFrameSize(0.2, true);
	
	// set size of normals
	object->setNormalsProperties(0.01, cColorf(1.0, 0.0, 0.0, 1.0), true);
	
	// render graphically both sides of triangles
	object->setUseCulling(false);
	
	// update global position
	object->computeGlobalPositions();
	
	// success
	return (0);
}

//---------------------------------------------------------------------------

void updateCameraPosition()
{
    // check values
    if (cameraDistance < 0.1) { cameraDistance = 0.1; }
    if (cameraAngleV > 89) { cameraAngleV = 89; }
    if (cameraAngleV < -89) { cameraAngleV = -89; }

    // compute position of camera in space
    cVector3d pos = cAdd(
                        cameraPosition,
                        cVector3d(
                            cameraDistance * cCosDeg(cameraAngleH) * cCosDeg(cameraAngleV),
                            cameraDistance * cSinDeg(cameraAngleH) * cCosDeg(cameraAngleV),
                            cameraDistance * cSinDeg(cameraAngleV)
                        )
                    );

    // compute lookat position
    cVector3d lookat = cameraPosition;

    // define role orientation of camera
    cVector3d up(0.0, 0.0, 1.0);

    // set new position to camera
    camera->set(pos, lookat, up);

    // recompute global positions
    world->computeGlobalPositions(true);

    // update tool position
    //if (tool != NULL)
    //tool->setPos(-cameraDistance, 0.0, 0.0);
}

//---------------------------------------------------------------------------
bool neightbor(int i, int j)
{
	if( abs(route[0][i]-route[0][j]) <= 1 &&
		abs(route[1][i]-route[1][j]) <= 1
		)
		return true;
	return false;
}

inline ULONGLONG time_ms()
{
	SYSTEMTIME systemTime;
	GetSystemTime( &systemTime );
	
	FILETIME fileTime;
	SystemTimeToFileTime( &systemTime, &fileTime );
	
	ULARGE_INTEGER uli;
	uli.LowPart = fileTime.dwLowDateTime;
	uli.HighPart = fileTime.dwHighDateTime;
	
	return (uli.QuadPart/10000) ;
}